//
//  Convert.hpp
//  markerARLib
//
//  Created by CADCG on 16/4/20.
//  Copyright © 2016年 CvidKal. All rights reserved.
//

#ifndef Convert_hpp
#define Convert_hpp

#include <stdio.h>
#include <opencv2/opencv.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

typedef Eigen::Matrix<double,3,3,Eigen::ColMajor>                               Matrix3D;

typedef Eigen::Matrix<double,3,1,Eigen::ColMajor>                               Vector3D;
namespace Converter
{
    
//    cv::Point2f Eigen2CV(const Eigen::Vector2f&);
//    Eigen::Matrix3d CV2Eigen(const cv::Matx33d&);
//    Eigen::Vector3d CV2Eigen(const cv::Matx31d&a);
    Matrix3D toMatrix3D(const cv::Matx33d&);
    Vector3D toVector3D(const cv::Matx31d&);
//    cv::Matx33d Eigen2CV(const Eigen::Matrix3d &a);
    void Eigen2CV(const Eigen::Vector2d &a,cv::Point2f&);
    void Eigen2CV(const Eigen::Vector3d &a,cv::Point3f&);
    template<typename TypeE,typename TypeC,int rows,int cols>
    Eigen::Matrix<TypeE,rows, cols>CV2Eigen(const cv::Mat&a)
    {
        Eigen::Matrix<TypeE, rows, cols> ret;
        for (int i=0;i<rows; ++i) {
            for (int j=0; j<cols;++j) {
                ret(i,j) =  a.ptr<TypeC>(i)[j];
            }
        }
        return ret;
    }
    
    template<typename T1,int rows,int cols>
    cv::Matx<T1,rows,cols> Eigen2CV(const Eigen::Matrix<T1, rows, cols>&a)
    {
        cv::Matx<T1,rows,cols> b;
        for(int i=0;i<rows;++i)
        {
            for(int j=0;j<cols;++j)
            {
                b(i,j) = a(i,j);
            }
        }
        return b;
    }
    
    
    template<typename T1,int rows,int cols>
    Eigen::Matrix<T1, rows, cols> CV2Eigen(const cv::Matx<T1,rows,cols>&a )
    {
        Eigen::Matrix<T1, rows, cols> b;
        for(int i=0;i<rows;++i)
        {
            for(int j=0;j<cols;++j)
            {
                b(i,j) = a(i,j);
            }
        }
        return b;
    }
    
//    template<typename T1,typename T2,int dims>
//    cv::Point_<T1,dims> Eigen2CV(const Eigen::Matrix<T2,dims,1>&a)
//    {
//        cv::Point_<T1,dims> b;
//    }
}




#endif /* Convert_hpp */
